#!/usr/bin/env python
import roslib; roslib.load_manifest('ardrone_joy_control')
import rospy
from teleop_msgs.msg import State
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist



class node (object):
    def __init__ (self):
        rospy.init_node('ardrone_joy_control_node', anonymous=True)
        self.takeoff_button_number =  rospy.get_param('~takeoff_button_number',0)
        self.landing_button_number=rospy.get_param('~landing_button_number',1)
        self.reset_button_number=rospy.get_param('~reset_button_number',2)
        self.throttle_as_lin_z=rospy.get_param('~throttle_as_lin_z',False)
        self.throttle_axis_number=rospy.get_param('~throttle_axis_number',3)
        self.max_lin_z=rospy.get_param('~max_lin_z',1.0)
        self.min_lin_z=rospy.get_param('~min_lin_z',-1.0)

        self.invert_lin_x=rospy.get_param('~invert_lin_x',False)
        self.invert_lin_y=rospy.get_param('~invert_lin_y',False)   
        self.invert_lin_z=rospy.get_param('~invert_lin_z',False)   
        self.invert_rot_z=rospy.get_param('~invert_rot_z',False)   
        
        if self.invert_lin_x:
            self.mult_lin_x=-1
        else:
            self.mult_lin_x=1
            
        if self.invert_lin_y:
            self.mult_lin_y=-1
        else:
            self.mult_lin_y=1
                        
        if self.invert_lin_z:
            self.mult_lin_z=-1
        else:
            self.mult_lin_z=1

        if self.invert_rot_z:
            self.mult_rot_z=-1
        else:
            self.mult_rot_z=1
                                
        
        self.pub_takeoff=rospy.Publisher('/ardrone/takeoff', Empty)
        self.pub_land =rospy.Publisher('/ardrone/land', Empty)
        self.pub_reset=rospy.Publisher('/ardrone/reset', Empty)
        self.pub_twist = rospy.Publisher('/cmd_vel', Twist)

        self.throttle_value=0
        rospy.Subscriber("/teleop_topic", State, self.process_teleop_topic)
        rospy.Subscriber("/ardrone_joy_control/cmd_vel", Twist, self.process_command_vel_topic)
        rospy.spin()

    def process_teleop_topic(self,data):
        if self.throttle_as_lin_z:
            if not data.axes[self.throttle_axis_number].value == self.throttle_value:
                self.throttle_value=(data.axes[self.throttle_axis_number].value+1)*((self.max_lin_z-self.min_lin_z)/(2))+self.min_lin_z
        if data.buttons[self.takeoff_button_number].value:
            self.pub_takeoff.publish(Empty())
            rospy.loginfo(rospy.get_name()+"Ardrone Takeoffing")
        if data.buttons[self.landing_button_number].value:
            self.pub_land.publish(Empty())
            rospy.loginfo(rospy.get_name()+"Ardrone Landing")
        if data.buttons[self.reset_button_number].value:
            self.pub_reset.publish(Empty())
            rospy.loginfo(rospy.get_name()+"Ardrone Falling")		
            
    def process_command_vel_topic(self,data):
        if self.throttle_as_lin_z:
            data.linear.z=self.throttle_value
        data.linear.x=self.mult_lin_x*data.linear.x            
        data.linear.y=self.mult_lin_y*data.linear.y
        data.linear.z=self.mult_lin_z*data.linear.z    
        data.angular.z=self.mult_rot_z*data.angular.z
        self.pub_twist.publish(data)
        #rospy.loginfo(rospy.get_name()+"Ardrone Falling")	

if __name__ == '__main__':
    my_node=node()


